#include "config.h"
#include "MultiWii.h"
#include "def.h"
#include "mission.h"
#include "timer.h"
#include "types.h"

play play_mission;

void mission_check() {
	if (play_mission.mission_step == 0 && rcData[AUX2] > 1700) {
		if (play_mission.mission) { //存在任务，使能定高
			f.BARO_MODE = 1;
			f.THROTTLE_IGNORED = 1;
			play_mission.mission_step = 1;//使能
			LED1_ON;
		}
	}
	if (play_mission.mission && rcData[AUX2] < 1300)//未使能任务重置任务步奏
	{
		LED1_OFF;
		if (play_mission.mission_step) { //存在任务，使能定高
			f.BARO_MODE = 0;
			f.THROTTLE_IGNORED = 0;
			play_mission.mission_step = 0;//失能任务
			play_mission.mission = 0;
			mission_led(0);
		}

	}
	if (!f.ARMED && rcData[AUX2] < 1300) {
		if ((play_mission.aux_last > 1300) && (play_mission.aux_last < 1700) && !play_mission.aux_flag)//上一状态剧中
		{
			play_mission.aux_flag = 1;
		}
		if (play_mission.aux_flag) {
			if (rcData[AUX1] < 1300) {
				play_mission.mission--;
				play_mission.aux_flag = 0;
			}
			if (rcData[AUX1] > 1700) {
				play_mission.mission++;
				play_mission.aux_flag = 0;
			}
			if (play_mission.mission > MISSION_MAX) {
				play_mission.mission = 0;
			}
			f.LAND_COMPLETED = 0;
		}
		play_mission.aux_last = rcData[AUX1];
	}
}

void mission_control() {
	if (play_mission.mission) {
		if (play_mission.mission_step == 0)//未开始任务时显示任务号
		{
			mission_led(play_mission.mission);
		} else {
			mission_led(play_mission.mission_step);//任务中显示步奏
		}
	} else {
		mission_led(0);
	}

	if (play_mission.mission == 1 && play_mission.mission_step != 0) { //任务一
		static uint8_t mission_flag = 0;
		static uint32_t mission_this_timer_mills = 0;
		openmv_enable = 1;
		if (play_mission.mission_step == 1) //第一步
		{
			initialThrottleHold = 1280;
			AltHold = 30;
			//* EDIT AltHold=TAKEOFF_ALT;//达到预计起飞高度
			// 启动高度
			play_mission.mission_step = 2;//执行下一步
			play_mission.time_ms_pre = millis();//记录当前时间
		} else if (play_mission.mission_step == 2) {
			if (alt.EstAlt > 10 && !f.OPTIC_MODE) {
				AltHold = TAKEOFF_ALT;
				f.OPTIC_MODE = 1; //启动光流
				OPTIC_ISUM[0] = 0;
				OPTIC_ISUM[1] = 0; // 重置光流积分，避免跑飞
			}
			if (TAKEOFF_ALT - alt.EstAlt <= 25) {
				AltHold += 10; // 缓慢提升高度
				play_mission.mission_step = 3; // 进入悬停
				play_mission.time_ms_pre = millis();//记录时间
			}
		} else if (play_mission.mission_step == 3)//hold on alt for 5 sec
		{
			mission_this_timer_mills = millis();
			if (mission_this_timer_mills - play_mission.time_ms_pre > 1000) {
				mission_flag++;
				play_mission.time_ms_pre = millis();
				if (mission_flag >= 3)//达到悬停时间
				{
					mission_flag = 0;
					play_mission.mission_step = 5;//交付降落程序
					play_mission.time_ms_pre = millis();
				}
			}
		}
	}
	if (play_mission.mission == 3 && play_mission.mission_step != 0) { //任务三
		static uint8_t mission_flag, my_rotate = 0, optic_p_value = 0;
		openmv_enable = 1;
		if (play_mission.mission_step == 1) //第一步
		{
			initialThrottleHold = 1310;
			//* EDIT AltHold=TAKEOFF_ALT;//达到预计起飞高度
			AltHold = 45; // 启动高度
			play_mission.mission_step = 2;//执行下一步
			play_mission.time_ms_pre = millis();//记录当前时间
		} else if (play_mission.mission_step == 2) {
			if (alt.EstAlt > 10 && !f.OPTIC_MODE)//如果当前高度大于10cm且光流没开
			{
				f.OPTIC_MODE = 1; //启动光流
				OPTIC_ISUM[0] = 0;
				OPTIC_ISUM[1] = 0; // 重置光流积分，避免跑飞
			}
			if (AltHold - alt.EstAlt <= 15) {
				AltHold += 15; // 缓慢提升高度
				if (AltHold - TAKEOFF_ALT > 0) { // 达到给定高度
					play_mission.mission_step = 3; // 进入旋转模式
					mission_flag = 200;
				}
				play_mission.time_ms_pre = millis();//记录时间
			}
		} else if (play_mission.mission_step == 3) {//步骤三
			// 任务 3: 旋转
			// 等待上升结束
			if (mission_flag == 200) {
				if (millis() - play_mission.time_ms_pre > 1500) {
					mission_flag = 0;
					play_mission.time_ms_pre = millis();
				}
			} else {
				if (my_rotate < 4) {//旋转次数
					int16_t angle_dif = 0;//角度误差

					if (mission_flag == 0) {
						magHold += 90;
						if (magHold <= -180) magHold += 360;
						if (magHold >= 180) magHold -= 360;
						// 设置目标：旋转90度
						mission_flag = 1; // 设置旋转标志
					}
					angle_dif = att.heading - magHold;//当前-期望
					if (angle_dif <= -180) angle_dif += 360;
					if (angle_dif >= 180) angle_dif -= 360;
					if (ABS(angle_dif) < 5) {
						if (mission_flag == 1) {
							play_mission.time_ms_pre = millis();
							BEEP_ON;
							mission_flag = 2;
						}
					}
					unsigned int now = millis();
					unsigned int timeDiff = now - play_mission.time_ms_pre;
					if (timeDiff > 2000 && mission_flag == 2) {
						// 通知完成，进入下一个旋转步骤
						play_mission.time_ms_pre = millis();
						mission_flag = 0;
						BEEP_OFF;
						++my_rotate;
					}
				} else {
					// 任务完成，交付降落
					if (mission_flag == 0) {
						play_mission.mission_step = 5;
						mission_flag = 255;
						play_mission.time_ms_pre = millis();
					}
				}
			}
		}
	}
}
